#include "DM4310.h"
#include "main.h"


extern FDCAN_HandleTypeDef hfdcan1;
int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
	float span = x_max - x_min;
	float offset = x_min;
	return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}

float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
	float span = x_max - x_min;
	float offset = x_min;
	return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

// 使能DM电机
/**
************************************************************************
* @brief:      	DM_enable_motor_mode: 启用电机模式函数
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要开启的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送启用特定模式的命令
************************************************************************
**/
void DM_enable_motor_mode(uint16_t motor_id, uint16_t mode_id)
{
    FDCAN_TxHeaderTypeDef TxMessage;
    static uint8_t    TxMessageData[8];
    TxMessage.Identifier= motor_id + mode_id;              //32位ID
    TxMessage.IdType=FDCAN_STANDARD_ID;                  //标准ID
    TxMessage.TxFrameType=FDCAN_DATA_FRAME;              //数据帧
    TxMessage.DataLength=FDCAN_DLC_BYTES_8;              //数据长度8bit
    TxMessage.ErrorStateIndicator=FDCAN_ESI_ACTIVE;            
    TxMessage.BitRateSwitch=FDCAN_BRS_OFF;               //关闭速率切换
    TxMessage.FDFormat=FDCAN_CLASSIC_CAN;                //传统的CAN模式
    TxMessage.TxEventFifoControl=FDCAN_NO_TX_EVENTS;     //无发送事件
    TxMessage.MessageMarker=0; 
    TxMessageData[0] = 0xFF;
    TxMessageData[1] = 0xFF;
    TxMessageData[2] = 0xFF;
    TxMessageData[3] = 0xFF;
    TxMessageData[4] = 0xFF;
    TxMessageData[5] = 0xFF;
    TxMessageData[6] = 0xFF;
    TxMessageData[7] = 0xFC;
    HAL_FDCAN_AddMessageToTxFifoQ(&DM_CAN,&TxMessage,TxMessageData);
}


// 失能DM电机
/**
************************************************************************
* @brief:      	DM_disable_motor_mode: 禁用电机模式函数
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要禁用的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送禁用特定模式的命令
************************************************************************
**/
void DM_disable_motor_mode(uint16_t motor_id, uint16_t mode_id)
{
    FDCAN_TxHeaderTypeDef TxMessage;
    static uint8_t    TxMessageData[8];
    TxMessage.Identifier= motor_id + mode_id;              //32位ID
    TxMessage.IdType=FDCAN_STANDARD_ID;                  //标准ID
    TxMessage.TxFrameType=FDCAN_DATA_FRAME;              //数据帧
    TxMessage.DataLength=FDCAN_DLC_BYTES_8;              //数据长度8bit
    TxMessage.ErrorStateIndicator=FDCAN_ESI_ACTIVE;            
    TxMessage.BitRateSwitch=FDCAN_BRS_OFF;               //关闭速率切换
    TxMessage.FDFormat=FDCAN_CLASSIC_CAN;                //传统的CAN模式
    TxMessage.TxEventFifoControl=FDCAN_NO_TX_EVENTS;     //无发送事件
    TxMessage.MessageMarker=0; 
    TxMessageData[0] = 0xFF;
    TxMessageData[1] = 0xFF;
    TxMessageData[2] = 0xFF;
    TxMessageData[3] = 0xFF;
    TxMessageData[4] = 0xFF;
    TxMessageData[5] = 0xFF;
    TxMessageData[6] = 0xFF;
    TxMessageData[7] = 0xFD;

    HAL_FDCAN_AddMessageToTxFifoQ(&DM_CAN,&TxMessage,TxMessageData);
}
/**
************************************************************************
* @brief:      	DM_save_pos_zero: 保存位置零点函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要保存位置零点的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送保存位置零点的命令
************************************************************************
**/
void DM_save_pos_zero(uint16_t motor_id, uint16_t mode_id)
{
    FDCAN_TxHeaderTypeDef TxMessage;
    static uint8_t    TxMessageData[8];
    TxMessage.Identifier= motor_id + mode_id;              //32位ID
    TxMessage.IdType=FDCAN_STANDARD_ID;                  //标准ID
    TxMessage.TxFrameType=FDCAN_DATA_FRAME;              //数据帧
    TxMessage.DataLength=FDCAN_DLC_BYTES_8;              //数据长度8bit
    TxMessage.ErrorStateIndicator=FDCAN_ESI_ACTIVE;            
    TxMessage.BitRateSwitch=FDCAN_BRS_OFF;               //关闭速率切换
    TxMessage.FDFormat=FDCAN_CLASSIC_CAN;                //传统的CAN模式
    TxMessage.TxEventFifoControl=FDCAN_NO_TX_EVENTS;     //无发送事件
    TxMessage.MessageMarker=0; 
    TxMessageData[0] = 0xFF;
    TxMessageData[1] = 0xFF;
    TxMessageData[2] = 0xFF;
    TxMessageData[3] = 0xFF;
    TxMessageData[4] = 0xFF;
    TxMessageData[5] = 0xFF;
    TxMessageData[6] = 0xFF;
    TxMessageData[7] = 0xFE;

    HAL_FDCAN_AddMessageToTxFifoQ(&DM_CAN,&TxMessage,TxMessageData);
}

/**
  * @brief:       DM_mit_ctrl: MIT模式下的电机控制函数
  * @param[in]:   hcan:	    指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
  * @param[in]:   motor_id:	电机ID，指定目标电机
  * @param[in]:   pos:		位置给定值
  * @param[in]:   vel:		速度给定值
  * @param[in]:   kp:		位置比例系数
  * @param[in]:   kd:		位置微分系数
  * @param[in]:   torq:		转矩给定值
  * @retval:      void
  * @details:     通过CAN总线向电机发送MIT模式下的控制帧。
  */
void DM_mit_ctrl(uint16_t motor_id, float pos, float vel,float kp, float kd, float torq)
{
	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
	uint16_t id = motor_id + MIT_MODE;

	pos_tmp = float_to_uint(pos,  P_MIN,  P_MAX,  16);
	vel_tmp = float_to_uint(vel,  V_MIN,  V_MAX,  12);
	kp_tmp  = float_to_uint(kp,   KP_MIN, KP_MAX, 12);
	kd_tmp  = float_to_uint(kd,   KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(torq, T_MIN,  T_MAX,  12);

    FDCAN_TxHeaderTypeDef TxMessage;
    static uint8_t    TxMessageData[8];
    TxMessage.Identifier=id;              //32位ID
    TxMessage.IdType=FDCAN_STANDARD_ID;                  //标准ID
    TxMessage.TxFrameType=FDCAN_DATA_FRAME;              //数据帧
    TxMessage.DataLength=FDCAN_DLC_BYTES_8;              //数据长度8bit
    TxMessage.ErrorStateIndicator=FDCAN_ESI_ACTIVE;            
    TxMessage.BitRateSwitch=FDCAN_BRS_OFF;               //关闭速率切换
    TxMessage.FDFormat=FDCAN_CLASSIC_CAN;                //传统的CAN模式
    TxMessage.TxEventFifoControl=FDCAN_NO_TX_EVENTS;     //无发送事件
    TxMessage.MessageMarker=0; 
    TxMessageData[0] = (pos_tmp >> 8);
    TxMessageData[1] = pos_tmp;
    TxMessageData[2] = (vel_tmp >> 4);
    TxMessageData[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
    TxMessageData[4] = kp_tmp;
    TxMessageData[5] = (kd_tmp >> 4);
    TxMessageData[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
    TxMessageData[7] = tor_tmp;
	
    HAL_FDCAN_AddMessageToTxFifoQ(&DM_CAN,&TxMessage,TxMessageData);
}
